#include "MBMatrix3x3.h"
#include "MBDataTypes.h"

#include <iostream>
#include <iomanip>

using std::cout;
using std::endl;
using std::cout;
using std::setw;

using cv::Mat;

/**
  *	Constructs a new MBMatrix3x3 with one value in every element at the
  * diagonal
  *
  * @param diagonalElem value that is used in every element at the diagonal
  */
MBMatrix3x3::MBMatrix3x3( const double &diagonalElem ) :
	mMatrix( diagonalElem * Mat::eye( 3, 3, CV_64F ) ) {};
	
/**
  *	Constructs a new MBMatrix3x3 with the nine values of the given array
  *
  * @param values const pointer to a double array containing the nine numbers of the matrix line by line
  */
MBMatrix3x3::MBMatrix3x3( const double *values ) :
	mMatrix( Mat( 3, 3, CV_64F, const_cast<double*>(values) ).clone() ) {};

/**
  *	Constructs a new MBMatrix3x3 with the nine given values
  *
  * @param v11 value at row 1, column 1
  * @param v12 value at row 1, column 2
  * @param v13 value at row 1, column 3
  * @param v21 value at row 2, column 1
  * @param v22 value at row 2, column 2
  * @param v23 value at row 2, column 3
  * @param v31 value at row 3, column 1
  * @param v32 value at row 3, column 2
  * @param v33 value at row 3, column 3
  */
MBMatrix3x3::MBMatrix3x3( const double &v11, const double &v12, const double &v13,
			 const double &v21, const double &v22, const double &v23,
			 const double &v31, const double &v32, const double &v33 )
{
	double values[] = { v11, v12, v13, v21, v22, v23, v31, v32, v33 };
	mMatrix = Mat( 3, 3, CV_64F, values ).clone();
}

/**
  *	Constructs a new MBMatrix3x3 object with the given cv::Mat matrix
  *
  * @param matrix cv::Mat matrix with the values of the new MBMatrix3x3 object
  */
MBMatrix3x3::MBMatrix3x3( const cv::Mat &matrix )
{
	mMatrix = matrix;
}

/**
  *	Returns the value at the given row and column
  *
  * @param row row of the element that is returned
  * @param col column of the element that is returned
  * @return value at the given row and column
  */
const double &MBMatrix3x3::At( const MBuint &row, const MBuint &col ) const
{
	return mMatrix.at<double>( static_cast<int>(row), static_cast<int>(col) );
}

const cv::Mat &MBMatrix3x3::GetCVMatrix( void ) const
{
	return mMatrix;
}

/**
  *	Prints the matrix to the console
  */
void MBMatrix3x3::Print( void ) const
{
	for (int row = 0; row < 3; row++)
	{
		for (int col = 0; col < 3; col++)
		{
			cout << setw( 10 ) << mMatrix.at<double>( row, col );
		}
		cout << endl;
	}
	//cout << "Size: " << sizeof( *this ) << endl;
}

/**
  *	Left-multiplies this matrix with another one and returns the result. This matrix
  * is not modified by this operation.
  *
  * @param other matrix that is right-multiplied to this one
  * @return product of this matrix and the other matrix
  */
MBMatrix3x3 MBMatrix3x3::operator*( const MBMatrix3x3 &other ) const
{
	return MBMatrix3x3( mMatrix * other.mMatrix );
}